package proyecto1IA;

import java.awt.Point;
import java.util.ArrayList;

public class CostoUniforme {

    private int matrizPesos[][];
    private Point robot, objeto1, objeto2, lugar1, lugar2;
    private boolean encontradoObj1, encontradoObj2, entregadoLugar1, entregadoLugar2;
    private ArrayList<ArrayList<Point>> solucion;
    private int profundidad;
    private int nodos;

    public CostoUniforme(int[][] matrizPesos, Point robot, Point objeto1, Point objeto2, Point lugar1, Point lugar2) {
        this.matrizPesos = matrizPesos;
        this.robot = robot;
        this.objeto1 = objeto1;
        this.objeto2 = objeto2;
        this.lugar1 = lugar1;
        this.lugar2 = lugar2;
        encontradoObj1 = false;
        encontradoObj2 = false;
        entregadoLugar1 = false;
        entregadoLugar2 = false;
        profundidad = 0;
        nodos = 1;
    }

    public ArrayList<ArrayList<Point>> controladorCostoUniforme() {

        for (int i = 0; i < matrizPesos.length; i++) {
            for (int j = 0; j < matrizPesos.length; j++) {
                imprimirSinSaltoDeLinea(matrizPesos[i][j]);
            }
            imprimir("");
        }

        Estado actual = new Estado(robot, new Point(-1, -1), 0);
        ArrayList<Estado> raizRobot = new ArrayList();
        raizRobot.add(actual);

        //Recoge alguno de los 2 objetos
        solucion = new ArrayList<ArrayList<Point>>();
        solucion.add(buscarObjetos(new ArrayList<Estado>(), raizRobot, new ArrayList<Estado>()));
        solucion.get(0).remove(solucion.get(0).size() - 1);

        //Si recogió el objeto 1 mira si puede entregarlo o ir por el otro objeto 2
        if (encontradoObj1) {
            //imprimir("ENcontradoObj1");
            actual = new Estado(robot, new Point(-1, -1), 0);
            raizRobot = new ArrayList();
            raizRobot.add(actual);

            solucion.add(buscarObjetivos(new ArrayList<Estado>(), raizRobot, new ArrayList<Estado>(), objeto2, lugar1));
            solucion.get(1).remove(solucion.get(1).size() - 1);
        } //Si recogió el 2 objeto mira si puede entregarlo o ir por el otro objeto 1
        else if (encontradoObj2) {
            //imprimir("ENcontradoObj2");
            actual = new Estado(robot, new Point(-1, -1), 0);
            raizRobot = new ArrayList();
            raizRobot.add(actual);
            //imprimir("Buscando objeto 1 o lugar 2, posición del robot: " + robot);
            solucion.add(buscarObjetivos(new ArrayList<Estado>(), raizRobot, new ArrayList<Estado>(), objeto1, lugar2));
            solucion.get(1).remove(solucion.get(1).size() - 1);
            //imprimir("ENcontradoObj2 = " + encontradoObj2);
            //imprimir("confirmado encontradoOBJ2");
        }

        //Revisa si encontró un objeto o si lo entregó
        if (encontradoObj1 && entregadoLugar1) {
            //imprimir("ENcontradosObj1 y EntregadoObj1");
            raizRobot = new ArrayList();
            actual = new Estado(robot, new Point(-1, -1), 0);
            raizRobot.add(actual);
            solucion.add(buscarObjetivos(new ArrayList<Estado>(), raizRobot, new ArrayList<Estado>(), objeto2, objeto2));
            solucion.get(2).remove(solucion.get(2).size() - 1);

        } else if (encontradoObj2 && entregadoLugar2) {
            //imprimir("ENcontradosObj2 y EntregadoObj2");
            raizRobot = new ArrayList();
            actual = new Estado(robot, new Point(-1, -1), 0);
            raizRobot.add(actual);
            solucion.add(buscarObjetivos(new ArrayList<Estado>(), raizRobot, new ArrayList<Estado>(), objeto1, objeto1));
            solucion.get(2).remove(solucion.get(2).size() - 1);

        } else if (encontradoObj1 && encontradoObj2) {
            //imprimir("ENcontradosObj1 y ENcontradoObj2");
            raizRobot = new ArrayList();
            actual = new Estado(robot, new Point(-1, -1), 0);
            raizRobot.add(actual);
            //imprimir("lugar 1: " + lugar1 + " lugar 2: "+ lugar2);
            solucion.add(buscarObjetivos(new ArrayList<Estado>(), raizRobot, new ArrayList<Estado>(), lugar1, lugar2));
            solucion.get(2).remove(solucion.get(2).size() - 1);
        }

        //Busca la última meta a la que levar el objeto restante
        if (entregadoLugar1) {
            //imprimir("EntregadoObj1");
            raizRobot = new ArrayList();
            actual = new Estado(robot, new Point(-1, -1), 0);
            raizRobot.add(actual);
            solucion.add(buscarObjetivos(new ArrayList<Estado>(), raizRobot, new ArrayList<Estado>(), lugar2, lugar2));
            
        } else if (entregadoLugar2) {
            //imprimir("EntregadoObj2");
            raizRobot = new ArrayList();
            actual = new Estado(robot, new Point(-1, -1), 0);
            raizRobot.add(actual);
            solucion.add(buscarObjetivos(new ArrayList<Estado>(), raizRobot, new ArrayList<Estado>(), lugar1, lugar1));
            
        }

        return solucion;
    }

    public ArrayList<Point> buscarObjetos(ArrayList<Estado> nodosExpandidos, ArrayList<Estado> colaARevisar, ArrayList<Estado> colaRevisados) {
        //Incializador pienses como si lo siguiente fuera como empezar un int=0
        Estado ARevision = new Estado(new Point(-1, -1), new Point(-1, -1), -1);
        int menor = Integer.MAX_VALUE;
        for (int posicion = (colaARevisar.size() - 1); posicion >= 0; posicion--) {
            if (!(estaEnLista(nodosExpandidos, colaARevisar.get(posicion))) || nodosExpandidos.isEmpty()) {
                nodosExpandidos.add(colaARevisar.get(posicion));
                nodos++;
            }
            imprimirSinSaltoDeLinea("Punto: " + colaARevisar.get(posicion).getPunto() + " Peso: " + colaARevisar.get(posicion).getPeso());
            imprimir("");
            if (!(estaEnLista(colaRevisados, colaARevisar.get(posicion)))) {
                int pesoARevisar = colaARevisar.get(posicion).getPeso();
                if (pesoARevisar < menor) {
                    menor = pesoARevisar;
                    ARevision = colaARevisar.get(posicion);
                }
            }
        }

        imprimirSinSaltoDeLinea("Se escogió: " + ARevision.getPunto() + " de peso: " + ARevision.getPeso());
        imprimir("");


        if (!(ARevision.getPunto().equals(objeto1) || ARevision.getPunto().equals(objeto2))) {

            colaARevisar.remove(ARevision);
            colaRevisados.add(ARevision);
            //Arriba
            if (!(ARevision.getPunto().x - 1 < 0)) {
                if (!estaEnLista(colaARevisar, new Estado(new Point(ARevision.getPunto().x - 1, ARevision.getPunto().y), new Point(), 0))) {
                    colaARevisar.add(new Estado(new Point(ARevision.getPunto().x - 1, ARevision.getPunto().y), ARevision.getPunto(), matrizPesos[ARevision.getPunto().x - 1][ARevision.getPunto().y] + ARevision.getPeso()));
                }
            }
            //Derecha
            if (!(ARevision.getPunto().y + 1 == matrizPesos.length)) {
                if (!estaEnLista(colaARevisar, new Estado(new Point(ARevision.getPunto().x, ARevision.getPunto().y + 1), new Point(), 0))) {
                    colaARevisar.add(new Estado(new Point(ARevision.getPunto().x, ARevision.getPunto().y + 1), ARevision.getPunto(), matrizPesos[ARevision.getPunto().x][ARevision.getPunto().y + 1] + ARevision.getPeso()));
                }
            }
            //Abajo
            if (!(ARevision.getPunto().x + 1 == matrizPesos.length)) {
                if (!estaEnLista(colaARevisar, new Estado(new Point(ARevision.getPunto().x + 1, ARevision.getPunto().y), new Point(), 0))) {
                    colaARevisar.add(new Estado(new Point(ARevision.getPunto().x + 1, ARevision.getPunto().y), ARevision.getPunto(), matrizPesos[ARevision.getPunto().x + 1][ARevision.getPunto().y] + ARevision.getPeso()));
                }
            }
            //Izquierda
            if (!(ARevision.getPunto().y - 1 < 0)) {
                if (!estaEnLista(colaARevisar, new Estado(new Point(ARevision.getPunto().x, ARevision.getPunto().y - 1), new Point(), 0))) {
                    colaARevisar.add(new Estado(new Point(ARevision.getPunto().x, ARevision.getPunto().y - 1), ARevision.getPunto(), matrizPesos[ARevision.getPunto().x][ARevision.getPunto().y - 1] + ARevision.getPeso()));
                }
            }
        } else {

            //Revisa si el encontrado es el objeto1
            if (ARevision.getPunto().equals(objeto1)) {
                encontradoObj1 = true;
                robot = objeto1;
            } else if (ARevision.getPunto().equals(objeto2)) {
                encontradoObj2 = true;
                robot = objeto2;
            }
            ArrayList<Point> camino = new ArrayList();
            camino.add(ARevision.getPunto());

            while (!(ARevision.predecesor.equals(new Point(-1, -1)))) {

                camino.add(0, ARevision.getPredecesor());

                for (int i = nodosExpandidos.size() - 1; i >= 0; i--) {
                    if (ARevision.getPredecesor().equals(nodosExpandidos.get(i).getPunto())) {
                        ARevision = nodosExpandidos.get(i);
                        break;
                    }
                }
            }

            return camino;
        }

        return buscarObjetos(nodosExpandidos, colaARevisar, colaRevisados);
    }

    public ArrayList<Point> buscarObjetivos(ArrayList<Estado> nodosExpandidos, ArrayList<Estado> colaARevisar, ArrayList<Estado> colaRevisados, Point objetivo1, Point objetivo2) {

        //Incializador pienses como si lo siguiente fuera como empezar un int=0
        Estado ARevision = new Estado(new Point(-1, -1), new Point(-1, -1), -1);
        int menor = Integer.MAX_VALUE;
        for (int posicion = (colaARevisar.size() - 1); posicion >= 0; posicion--) {
            if (!(estaEnLista(nodosExpandidos, colaARevisar.get(posicion))) || nodosExpandidos.isEmpty()) {
                nodosExpandidos.add(colaARevisar.get(posicion));
                nodos++;
            }
            if (!(estaEnLista(colaRevisados, colaARevisar.get(posicion)))) {
                int pesoARevisar = colaARevisar.get(posicion).getPeso();
                if (pesoARevisar < menor) {
                    menor = pesoARevisar;
                    ARevision = colaARevisar.get(posicion);
                }

            }
        }


        if (!(ARevision.getPunto().equals(objetivo1) || ARevision.getPunto().equals(objetivo2))) {

            colaARevisar.remove(ARevision);
            colaRevisados.add(ARevision);
            //Arriba
            if (!(ARevision.getPunto().x - 1 < 0)) {
                if (!estaEnLista(colaARevisar, new Estado(new Point(ARevision.getPunto().x - 1, ARevision.getPunto().y), new Point(), 0))) {
                    colaARevisar.add(new Estado(new Point(ARevision.getPunto().x - 1, ARevision.getPunto().y), ARevision.getPunto(), matrizPesos[ARevision.getPunto().x - 1][ARevision.getPunto().y] + ARevision.getPeso()));
                }
            }
            //Derecha
            if (!(ARevision.getPunto().y + 1 == matrizPesos.length)) {
                if (!estaEnLista(colaARevisar, new Estado(new Point(ARevision.getPunto().x, ARevision.getPunto().y + 1), new Point(), 0))) {
                    colaARevisar.add(new Estado(new Point(ARevision.getPunto().x, ARevision.getPunto().y + 1), ARevision.getPunto(), matrizPesos[ARevision.getPunto().x][ARevision.getPunto().y + 1] + ARevision.getPeso()));
                }
            }
            //Abajo
            if (!(ARevision.getPunto().x + 1 == matrizPesos.length)) {
                if (!estaEnLista(colaARevisar, new Estado(new Point(ARevision.getPunto().x + 1, ARevision.getPunto().y), new Point(), 0))) {
                    colaARevisar.add(new Estado(new Point(ARevision.getPunto().x + 1, ARevision.getPunto().y), ARevision.getPunto(), matrizPesos[ARevision.getPunto().x + 1][ARevision.getPunto().y] + ARevision.getPeso()));
                }
            }
            //Izquierda
            if (!(ARevision.getPunto().y - 1 < 0)) {
                if (!estaEnLista(colaARevisar, new Estado(new Point(ARevision.getPunto().x, ARevision.getPunto().y - 1), new Point(), 0))) {
                    colaARevisar.add(new Estado(new Point(ARevision.getPunto().x, ARevision.getPunto().y - 1), ARevision.getPunto(), matrizPesos[ARevision.getPunto().x][ARevision.getPunto().y - 1] + ARevision.getPeso()));
                }
            }
        } else {

            //Revisa si el encontrado es el objeto1
            if (ARevision.getPunto().equals(objetivo1)) {

                if (objetivo1.equals(objeto1)) {
                    encontradoObj1 = true;
                    robot = objeto1;
                } else if (objetivo1.equals(objeto2)) {
                    encontradoObj2 = true;
                    robot = objeto2;
                } else if (objetivo1.equals(lugar1)) {
                    entregadoLugar1 = true;
                    robot = lugar1;
                } else if (objetivo1.equals(lugar2)) {
                    entregadoLugar2 = true;
                    robot = lugar2;
                }

            } else if (ARevision.getPunto().equals(objetivo2)) {

                if (objetivo2.equals(lugar1)) {
                    entregadoLugar1 = true;
                    robot = lugar1;
                } else if (objetivo2.equals(lugar2)) {
                    entregadoLugar2 = true;
                    robot = lugar2;
                }
            }
            ArrayList<Point> camino = new ArrayList();
            camino.add(ARevision.getPunto());

            while (!(ARevision.predecesor.equals(new Point(-1, -1)))) {

                camino.add(0, ARevision.getPredecesor());

                for (int i = nodosExpandidos.size() - 1; i >= 0; i--) {
                    if (ARevision.getPredecesor().equals(nodosExpandidos.get(i).getPunto())) {
                        ARevision = nodosExpandidos.get(i);
                        break;
                    }
                }
            }

            return camino;
        }

        return buscarObjetivos(nodosExpandidos, colaARevisar, colaRevisados, objetivo1, objetivo2);

    }

    public boolean estaEnLista(ArrayList<Estado> lista, Estado actual) {

        for (int recorrido = 0; recorrido < lista.size(); recorrido++) {
            if (!lista.isEmpty()) {
                if (lista.get(recorrido).getPunto().x == actual.getPunto().x
                        && lista.get(recorrido).getPunto().y == actual.getPunto().y) {
                    return true;
                }
            }
        }
        return false;

    }

    public void imprimir(Object mensaje) {

        System.out.println(mensaje);

    }

    public void imprimirSinSaltoDeLinea(Object mensaje) {

        System.out.print(mensaje + " ");

    }

    public int getProfundidad() {
        return profundidad;
    }

    public int getNodos() {
        return nodos;
    }
}
